#include "MPU6050_light.h"
#include "Arduino.h"

int chip_type = CHIP_MPU6050;

//角度限制，将角度限制在[-limit,limt]
static float wrap(float angle, float limit)
{
  while (angle > limit)
    angle -= 2 * limit;
  while (angle < -limit)
    angle += 2 * limit;
  return angle;
}

//初始化MPU6050,设置IIC总线连接
MPU6050::MPU6050(TwoWire &w)
{
  //指定为IIC通信
  wire = &w;
  //设置默认的滤波系数可以对陀螺仪的数据进行滤波处理
  setFilterGyroCoef(DEFAULT_GYRO_COEFF);
  //设置陀螺仪角速度的偏移量
  setGyroOffsets(0, 0, 0);
  //设置加速度计的偏移量
  setAccOffsets(0, 0, 0);
}

//向mpu6050写入数据
byte MPU6050::writeData(byte reg, byte data)
{
  //开始I2C传输会话，这是MPU6050的IIC通信地址
  wire->beginTransmission(address);
  //找到IIC的寄存器地址
  wire->write(reg);
  //向IIC的寄存器地址写入数据
  wire->write(data);
  //结束IIC通信
  byte status = wire->endTransmission();
  return status;
}

//从mpu6050读取数据
byte MPU6050::readData(byte reg)
{
  
  wire->beginTransmission(address);
  //告诉MPU6050接下来要读取reg寄存器的数据
  wire->write(reg);
  //保持对总线继续使用
  wire->endTransmission(true);
  //从address中读取一个字节的数据
  wire->requestFrom(address, (uint8_t)1);
  //将读到的数据放入data中
  byte data = wire->read();
  return data;
}

//设置角速度的测量范围
byte MPU6050::setGyroConfig(int config_num)
{
  byte status;
  switch (config_num)
  {
    //获得角速度的测量范围
  case 0:
    //测量范围在[-250,250]
    gyro_lsb_to_degsec = 131.0;
    status = writeData(MPU6050_GYRO_CONFIG_REGISTER, 0x00);
    break;
  case 1:
    //测量范围在[-500,500]
    gyro_lsb_to_degsec = 65.5;
    status = writeData(MPU6050_GYRO_CONFIG_REGISTER, 0x08);
    break;
  case 2: 
    //测量范围在[-1000,1000]
    gyro_lsb_to_degsec = 32.8;
    status = writeData(MPU6050_GYRO_CONFIG_REGISTER, 0x10);
    break;
  case 3:
    //测量范围在[-2000,2000]
    gyro_lsb_to_degsec = 16.4;
    status = writeData(MPU6050_GYRO_CONFIG_REGISTER, 0x18);
    break;
  default: // error
    status = 1;
    break;
  }
  return status;
}

//设置加速度的测量范围
byte MPU6050::setAccConfig(int config_num)
{
  byte status;
  switch (config_num)
  {
    //获得加速度的测量范围
  case 0: 
    //测量范围在[-2g,2g]
    acc_lsb_to_g = 16384.0;
    status = writeData(MPU6050_ACCEL_CONFIG_REGISTER, 0x00);
    break;
  case 1: 
    //测量范围在[-4g,4g]
    acc_lsb_to_g = 8192.0;
    status = writeData(MPU6050_ACCEL_CONFIG_REGISTER, 0x08);
    break;
  case 2: 
    //测量范围在[-8g,8g]
    acc_lsb_to_g = 4096.0;
    status = writeData(MPU6050_ACCEL_CONFIG_REGISTER, 0x10);
    break;
  case 3: 
    //测量范围在[-16g,16g]
    acc_lsb_to_g = 2048.0;
    status = writeData(MPU6050_ACCEL_CONFIG_REGISTER, 0x18);
    break;
  default: // error
    status = 1;
    break;
  }
  return status;
}

//更新数据
void MPU6050::update()
{
  //读取加速度，角速度，温度数据
  this->fetchData();

  //利用加速度计计算x方向和y方向的角度

  //首先确定z的方向
  float sgZ = accZ < 0 ? -1 : 1;

  //计算得到x的角度
  angleAccX = atan2(accY, sgZ * sqrt(accZ * accZ + accX * accX)) * RAD_2_DEG;
  //计算得到y的角度
  angleAccY = -atan2(accX, sqrt(accZ * accZ + accY * accY)) * RAD_2_DEG;

  //计算时间差
  unsigned long Tnew = millis();
  float dt = (Tnew - preInterval) * 1e-3;
  preInterval = Tnew;

  //得到x和y的角度
  angleX = wrap(filterGyroCoef * (angleAccX + wrap(angleX + gyroX * dt - angleAccX, 180)) + (1.0 - filterGyroCoef) * angleAccX, 180);
  angleY = wrap(filterGyroCoef * (angleAccY + wrap(angleY + sgZ * gyroY * dt - angleAccY, 90)) + (1.0 - filterGyroCoef) * angleAccY, 90);
  angleZ += gyroZ * dt; // not wrapped
}

//初始化数据
byte MPU6050::begin(int gyro_config_num, int acc_config_num)
{
  //将传感器地址设置为MPU6050的地址
  address = MPU6050_ADDR;
  //调用writeData，向MPU6050_PWR_MGMT_1_REGISTER写入0x01,表示唤醒MPU6050传感器
  //其中MPU6050_PWR_MGMT_1_REGISTER表示MPU6050的电源管理器
  byte status_mpu6050 = writeData(MPU6050_PWR_MGMT_1_REGISTER, 0x01);
  //将传感器地址设置为LSM6DS3_ADDR的地址
  address = LSM6DS3_ADDR;
  //读取LSM6DS3的WHO_AM_I寄存器的值，可以确定其是否链接上
  byte status_lsm6ds3 = readData(LSM6DS3_WHO_AM_I);

  //验证我们用的是MPU6050还是LSM6DS3
  byte status = status_mpu6050;
  if (status_mpu6050 == 0)
  {
    address = MPU6050_ADDR;
    chip_type = CHIP_MPU6050;
  }
  else if (status_lsm6ds3 == LSM6DS3_WHO_AM_I_VALUE)
  {
    status = 0;
    address = LSM6DS3_ADDR;
    chip_type = CHIP_LSM6DS3;
  }

  if (chip_type == CHIP_MPU6050)
  {
    //设置传感器的采样频率为默认频率
    writeData(MPU6050_SMPLRT_DIV_REGISTER, 0x00);
    //设置寄存器为默认的配置模式
    writeData(MPU6050_CONFIG_REGISTER, 0x00);

    //设置角速度的测量范围
    setGyroConfig(gyro_config_num);
    //设置加速度的测量范围
    setAccConfig(acc_config_num);

    //调用update函数，读取最新的角速度和加速度
    this->update();

    //通过加速度计计算出x和y的角度
    angleX = this->getAccAngleX();
    angleY = this->getAccAngleY();

    //获得当前的时间
    preInterval = millis();
  }

  if (chip_type == CHIP_LSM6DS3)
  {
    writeData(0x10, 0b01110001); // gyro_config_num  range = +- 500 deg/s
    gyro_lsb_to_degsec = 65.5;   //  +- 500 deg/s
    writeData(0x11, 0b01110100); // acc_config_num  range = +- 2 g
    acc_lsb_to_g = 16384.0;      // +- 2g
    // Serial1.println(readData(0x10), 2);
    // Serial1.println(readData(0x11), 2);
  }
  //返回状态
  return status;
}


//该函数用于设置陀螺仪的偏移量
void MPU6050::setGyroOffsets(float x, float y, float z)
{
  gyroXoffset = x;
  gyroYoffset = y;
  gyroZoffset = z;
}

//该函数用于设置加速度计的偏移量
void MPU6050::setAccOffsets(float x, float y, float z)
{
  accXoffset = x;
  accYoffset = y;
  accZoffset = z;
}

//设置陀螺仪的滤波系数
//loat gyro_coeff代表要设置的陀螺仪滤波系数
void MPU6050::setFilterGyroCoef(float gyro_coeff)
{
  //如果gyro_coeff大于1或者小于0,设置为默认系数为0.98
  if ((gyro_coeff < 0) or (gyro_coeff > 1))
  {
    gyro_coeff = DEFAULT_GYRO_COEFF;
  }
  //将设置好的滤波系数传递给filterGyroCoef，用于滤波处理
  filterGyroCoef = gyro_coeff;
}

//用于设置加速度计的滤波系数
void MPU6050::setFilterAccCoef(float acc_coeff)
{
  //调用了陀螺仪的滤波系数函数，表示他们俩个相加为1
  setFilterGyroCoef(1.0 - acc_coeff);
}

//计算并设置 MPU6050 传感器的加速度计和陀螺仪的偏移量
void MPU6050::calcOffsets(bool is_calc_gyro, bool is_calc_acc)
{
  //是否计算陀螺仪的偏移量
  if (is_calc_gyro)
  {
    setGyroOffsets(0, 0, 0);
  }

  //是否计算加速度的偏移量
  if (is_calc_acc)
  {
    setAccOffsets(0, 0, 0);
  }

  //用于累加加速度计和陀螺仪的数据，前三个是加速度的，后三个是陀螺仪的
  float ag[6] = {0, 0, 0, 0, 0, 0}; 

  //多次测量加速度和陀螺仪数据并且累加数据
  for (int i = 0; i < CALIB_OFFSET_NB_MES; i++)
  {
    this->fetchData();
    ag[0] += accX;
    ag[1] += accY;
    ag[2] += (accZ - 1.0);
    ag[3] += gyroX;
    ag[4] += gyroY;
    ag[5] += gyroZ;
    delay(1);
  }

  //计算并设置加速度计偏移量
  if (is_calc_acc)
  {
    //利用累加的数据除以测量次数，得到的是平均偏移量
    accXoffset = ag[0] / CALIB_OFFSET_NB_MES;
    accYoffset = ag[1] / CALIB_OFFSET_NB_MES;
    accZoffset = ag[2] / CALIB_OFFSET_NB_MES;
  }

  //计算并设置陀螺仪偏移量
  if (is_calc_gyro)
  {
    //利用累加的数据除以测量次数，得到的是平均偏移量
    gyroXoffset = ag[3] / CALIB_OFFSET_NB_MES;
    gyroYoffset = ag[4] / CALIB_OFFSET_NB_MES;
    gyroZoffset = ag[5] / CALIB_OFFSET_NB_MES;
  }
}

//从MPU6050读取加速度，陀螺仪和温度数据
void MPU6050::fetchData()
{
  //使用的是MPU6050芯片
  if (chip_type == CHIP_MPU6050)
  {
    wire->beginTransmission(address);
    wire->write(MPU6050_ACCEL_OUT_REGISTER);
    //结束当前传输会话，不释放总线
    wire->endTransmission(false);
    //向传感器请求了14个字节的数据，他们分别是加速度，陀螺仪和温度数据
    wire->requestFrom(address, (uint8_t)14);

    //用于存储加速度，陀螺仪和温度数据
    int16_t rawData[7];

    //循环七次，分别读取加速度，陀螺仪，温度数据
    for (int i = 0; i < 7; i++)
    {
      //先读出高八位的数据，然后左移八位
      rawData[i] = wire->read() << 8;
      //读取低八位的数据，进行合并
      rawData[i] |= wire->read();
    }

    //获得加速度数据，温度数据，陀螺仪数据
    accX = ((float)rawData[0]) / acc_lsb_to_g - accXoffset;
    accY = ((float)rawData[1]) / acc_lsb_to_g - accYoffset;
    accZ = (!upsideDownMounting - upsideDownMounting) * ((float)rawData[2]) / acc_lsb_to_g - accZoffset;
    temp = (rawData[3] + TEMP_LSB_OFFSET) / TEMP_LSB_2_DEGREE;
    gyroX = ((float)rawData[4]) / gyro_lsb_to_degsec - gyroXoffset;
    gyroY = ((float)rawData[5]) / gyro_lsb_to_degsec - gyroYoffset;
    gyroZ = ((float)rawData[6]) / gyro_lsb_to_degsec - gyroZoffset;
  }


  else if (chip_type == CHIP_LSM6DS3)
  {
    byte status = readData(0x1E);
    // 有温度了
    if (status & LSM6DS3_OUT_STATUS_MASK_TEMP)
    {
      wire->beginTransmission(address);
      wire->write(LSM6DS3_OUT_TEMP_REGISTER);
      wire->endTransmission(false);
      wire->requestFrom(address, (uint8_t)2);
      int16_t rawData;
      byte low = wire->read();
      byte high = wire->read();
      rawData = low | high << 8;
      temp = rawData / LSM6DS3_TEMP_LSB_PER_DEG + LSM6DS3_TEMP_OFFSET_DEG;
    }
    if (status & LSM6DS3_OUT_STATUS_MASK_G)
    {
      wire->beginTransmission(address);
      wire->write(LSM6DS3_OUT_G_REGISTER);
      wire->endTransmission(false);
      wire->requestFrom(address, (uint8_t)6);
      int16_t rawData[3];
      for (int i = 0; i < 3; i++)
      {
        byte low = wire->read();
        byte high = wire->read();
        rawData[i] = low | high << 8;
      }
      gyroX = ((float)rawData[0]) / gyro_lsb_to_degsec - gyroXoffset;
      gyroY = ((float)rawData[1]) / gyro_lsb_to_degsec - gyroYoffset;
      gyroZ = ((float)rawData[2]) / gyro_lsb_to_degsec - gyroZoffset;
    }
    if (status & LSM6DS3_OUT_STATUS_MASK_XL)
    {
      wire->beginTransmission(address);
      wire->write(LSM6DS3_OUT_XL_REGISTER);
      wire->endTransmission(false);
      wire->requestFrom(address, (uint8_t)6);
      int16_t rawData[3];
      for (int i = 0; i < 3; i++)
      {
        byte low = wire->read();
        byte high = wire->read();
        rawData[i] = low | high << 8;
      }
      accX = ((float)rawData[0]) / acc_lsb_to_g - accXoffset;
      accY = ((float)rawData[1]) / acc_lsb_to_g - accYoffset;
      accZ = (!upsideDownMounting - upsideDownMounting) * ((float)rawData[2]) / acc_lsb_to_g - accZoffset;
    }
  }
}
